
// CmdMessenger library available from https://github.com/dreamcat4/cmdmessenger
#include "CmdMessenger.h"

// Streaming4 library available from http://arduiniana.org/libraries/streaming/
#include "Streaming.h"

// Mustnt conflict / collide with our message payload data. Fine if we use base64 library ^^ above
char field_separator = ',';
char command_separator = ';';


char ledPin = 13;

int HorizontalServoPin = 3;
int VerticalServoPin = 5;

int RightFowardPin =6;
int RightBackwardPin =9;
int LeftFowarPin =11;
int LeftBackwarPin = 10;

#define channelCount 8
#define PWM_arrayLenght 4
int PWM_Array [PWM_arrayLenght]; //rightFront , roghtBack , LeftFront, LeftBack

// Attach a new CmdMessenger object to the default Serial port
CmdMessenger cmdMessenger = CmdMessenger(Serial, field_separator, command_separator);

// ------------------ C M D  L I S T I N G ( T X / R X ) ---------------------

// We can define up to a default of 50 cmds total, including both directions (send + recieve)
// and including also the first 4 default command codes for the generic error handling.
// If you run out of message slots, then just increase the value of MAXCALLBACKS in CmdMessenger.h

// Commands we send from the Arduino to be received on the PC
enum
{
	kCOMM_ERROR    = 000, // Lets Arduino report serial port comm error back to the PC (only works for some comm errors)
	kACK           = 001, // Arduino acknowledges cmd was received
	kARDUINO_READY = 002, // After opening the comm port, send this cmd 02 from PC to check arduino is ready
	kERR           = 003, // Arduino reports badly formatted cmd, or cmd not recognised
	ADC_DATA       = 004,

	kSEND_CMDS_END, // Mustnt delete this line
};

// Commands we send from the PC and want to recieve on the Arduino.
// We must define a callback function in our Arduino program for each entry in the list below vv.
// They start at the address kSEND_CMDS_END defined ^^ above as 004
messengerCallbackFunction messengerCallbacks[] = 
{
	MoveComand,            // 005 in this example
	SendADC,			   // 006 in this example
	NULL
};
// Its also possible (above ^^) to implement some symetric commands, when both the Arduino and
// PC / host are using each other's same command numbers. However we recommend only to do this if you
// really have the exact same messages going in both directions. Then specify the integers (with '=')


// ------------------ C A L L B A C K  M E T H O D S -------------------------

void MoveComand()
{
	//cmdMessenger.sendCmd(kACK,"Move Command has been recieved");
	int i =0;
	while ( cmdMessenger.available() )
	{

		if (i > PWM_arrayLenght) break; 
		PWM_Array[i]= cmdMessenger.readInt();
		i++;
	}
	
	ActualizeMovements();
	//cmdMessenger.sendCmd(kACK,"Move Command has been set");


}

void ActualizeMovements()
{
	/*for (int i = 0; i < 4; i++)
	{
		Serial.print(PWM_Array[i]);
		Serial.print(" ");
	}*/
	analogWrite(RightFowardPin, PWM_Array[0]);
	analogWrite(RightBackwardPin, PWM_Array[1]);
	analogWrite(LeftFowarPin, PWM_Array[2]);
	analogWrite(LeftBackwarPin, PWM_Array[3]);

}

// ------------------ D E F A U L T  C A L L B A C K S -----------------------

void arduino_ready()
{
	// In response to ping. We just send a throw-away Acknowledgement to say "im alive"
	cmdMessenger.sendCmd(kACK,"Arduino ready");
}

void unknownCmd()
{
	// Default response for unknown commands and corrupt messages
	cmdMessenger.sendCmd(kERR,"Unknown command");
}

// ------------------ E N D  C A L L B A C K  M E T H O D S ------------------



// ------------------ S E T U P ----------------------------------------------

void attach_callbacks(messengerCallbackFunction* callbacks)
{
	int i = 0;
	int offset = kSEND_CMDS_END;
	while(callbacks[i])
	{
		cmdMessenger.attach(offset+i, callbacks[i]);
		i++;
	}
}

void ResetControls()
{
	for (int i = 0; i < PWM_arrayLenght; i++)
	{
		PWM_Array[i]= 0;
	}
}

void setup() 
{
	// Listen on serial connection for messages from the pc
	Serial.begin(115200); // Arduino Uno, Mega, with AT8u2 USB

	// cmdMessenger.discard_LF_CR(); // Useful if your terminal appends CR/LF, and you wish to remove them
	cmdMessenger.print_LF_CR();   // Make output more readable whilst debugging in Arduino Serial Monitor

	// Attach default / generic callback methods
	cmdMessenger.attach(kARDUINO_READY, arduino_ready);
	cmdMessenger.attach(unknownCmd);

	// Attach my application's user-defined callback methods
	attach_callbacks(messengerCallbacks);

	arduino_ready();

	ResetControls();

	// blink
	pinMode(13, OUTPUT);
}


// ------------------ M A I N ( ) --------------------------------------------

// Timeout handling
long timeoutInterval = 2000; // 2 seconds
long previousMillis = 0;
int counter = 0;

void SendADC()
{
	String values = "";
	int voltange[channelCount];
	for (int i = 0; i < channelCount; i++)
	{
		values +=  analogRead(i);
		if(i < channelCount -1) values += field_separator;
	}
	char charArray [channelCount*5];
	values.toCharArray(charArray, channelCount*5);
	cmdMessenger.sendCmd(ADC_DATA, charArray );
}

void timeout()
{
	// blink
	if (counter % 2)
		digitalWrite(13, HIGH);
	else
		digitalWrite(13, LOW);
	counter ++;
}  

void loop() 
{
	// Process incoming serial data, if any
	cmdMessenger.feedinSerialData();

	// handle timeout function, if any
	if (  millis() - previousMillis > timeoutInterval )
	{
		timeout();
		previousMillis = millis();
		ResetControls();
		ActualizeMovements();
	}

	// Loop.
}

